/*
 * Copyright 2018 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_DATA_H_
#define CARTOGRAPHER_MAPPING_POSE_GRAPH_DATA_H_

#include <map>
#include <set>
#include <vector>

#include "cartographer/mapping/internal/optimization/optimization_problem_2d.h"
// #include
// "cartographer/mapping/internal/optimization/optimization_problem_3d.h"
#include "cartographer/mapping/id.h"
#include "cartographer/mapping/internal/trajectory_connectivity_state.h"
// #include "cartographer/mapping/pose_graph.h"
// #include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping/submaps.h"
#include "cartographer/transform/rigid_transform.h"

namespace cartographer {
namespace mapping {
// from 3d
struct InitialTrajectoryPose {
  int to_trajectory_id;
  transform::Rigid3d relative_pose;
  common::Time time;
};

// frome pose_graph_interface.h
struct Constraint {
  struct Pose {
    transform::Rigid3d zbar_ij;
    double translation_weight;
    double rotation_weight;
  };

  SubmapId submap_id;  // 'i' in the paper.
  NodeId node_id;      // 'j' in the paper.

  // Pose of the node 'j' relative to submap 'i'.
  Pose pose;

  // Differentiates between intra-submap (where node 'j' was inserted into
  // submap 'i') and inter-submap constraints (where node 'j' was not inserted
  // into submap 'i').
  enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
};

struct SubmapPose {
  int version;
  transform::Rigid3d pose;
};

struct SubmapData {
  std::shared_ptr<const Submap> submap;
  transform::Rigid3d pose;
};

struct TrajectoryData {
  double gravity_constant = 9.8;
  std::array<double, 4> imu_calibration{{1., 0., 0., 0.}};
  // absl::optional<transform::Rigid3d> fixed_frame_origin_in_map;
  std::optional<transform::Rigid3d> fixed_frame_origin_in_map;
};

// using GlobalSlamOptimizationCallback = std::function<void(
//     const std::map<int, SubmapId>&, const std::map<int, NodeId>&)>;

enum class TrajectoryState { ACTIVE, FINISHED, FROZEN, DELETED };
// The current state of the submap in the background threads. After this
// transitions to 'kFinished', all nodes are tried to match
// against this submap. Likewise, all new nodes are matched against submaps in
// that state.
enum class SubmapState { kNoConstraintSearch, kFinished };

struct InternalTrajectoryState {
  enum class DeletionState {
    NORMAL,
    SCHEDULED_FOR_DELETION,
    WAIT_FOR_DELETION
  };

  ::cartographer::mapping::TrajectoryState state =
      ::cartographer::mapping::TrajectoryState::ACTIVE;
  DeletionState deletion_state = DeletionState::NORMAL;
};

struct InternalSubmapData {
  std::shared_ptr<const Submap> submap;
  SubmapState state = SubmapState::kNoConstraintSearch;

  // IDs of the nodes that were inserted into this map together with
  // constraints for them. They are not to be matched again when this submap
  // becomes 'kFinished'.
  std::set<NodeId> node_ids;
};

namespace optimization {
struct NodeSpec2D;
struct SubmapSpec2D;
}  // namespace optimization

struct PoseGraphData {
  // Submaps get assigned an ID and state as soon as they are seen, even
  // before they take part in the background computations.
  MapById<SubmapId, InternalSubmapData> submap_data;

  // Global submap poses currently used for displaying data.
  MapById<SubmapId, optimization::SubmapSpec2D> global_submap_poses_2d;
  // MapById<SubmapId, optimization::SubmapSpec3D> global_submap_poses_3d;

  // Data that are currently being shown.
  MapById<NodeId, TrajectoryNode> trajectory_nodes;

  // Global landmark poses with all observations.
  // std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
  //     landmark_nodes;

  // How our various trajectories are related.
  TrajectoryConnectivityState trajectory_connectivity_state;
  int num_trajectory_nodes = 0;
  std::map<int, InternalTrajectoryState> trajectories_state;

  // Set of all initial trajectory poses.
  std::map<int, InitialTrajectoryPose> initial_trajectory_poses;

  std::vector<Constraint> constraints;
};

}  // namespace mapping
}  // namespace cartographer

#endif  // CARTOGRAPHER_MAPPING_POSE_GRAPH_DATA_H_
